/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package batailledesardennes.game.entity.task;

import batailledesardennes.game.entity.Entity;
import batailledesardennes.util.FastMath;
import batailledesardennes.util.geomFloat.Point;
import batailledesardennes.util.geomFloat.path.PathFinder;
import java.util.List;

/**
 *
 * @author charly
 */
public class MoveTask implements Task {
   protected final float ERR_ANGLE = 0.00001f;
   protected final float ERR_POS = 0.0001f;
   private Entity entity;
   private Point target;
   private List<Point> path;
   private float speed;
   private float angle;
   protected float maxSpeed;
   protected float accel;
   protected float rotationSpeed;
   protected float arrivalDist;

   public MoveTask() {
      accel = -1;
      rotationSpeed = -1;
      maxSpeed = 0.1f;
      arrivalDist = -1;
   }

   @Override
   public boolean update(int delta) {
      if (path == null) {
         calcPath();
         if (path == null ||path.isEmpty()) {
            return false;
         }
      }

      Point curTarget = path.get(0);
      float targetAngle = getSeekAngle(curTarget);
      float targetSpeed = getArrivalSpeed(curTarget);
      applyMovement(curTarget,targetAngle, targetSpeed, delta);

      if (entity.getPos().distanceSq(curTarget) < 1) {
         path.remove(0);
      }

      if (path.isEmpty()) {
         return false;
      }
      return true;
   }

   private void calcPath() {
      Point pos = entity.getPos();
      PathFinder pathFinder = entity.getEnv().getMap().getAccessHandler().getPathFinder();
      path = pathFinder.calcPath(pos, target);
   }

   protected float getSeekAngle(Point target) {
      return -entity.getPos().findSignedAngle(target);
   }

   protected float getArrivalSpeed(Point target) {
      if (arrivalDist <= 0) {
         return maxSpeed;
      }
      Point cur = entity.getPos();
      float dist = cur.distance(target);
      float threshold = arrivalDist * (accel > 0 ? 1 / accel : 1);
      if (dist > threshold) {
         return maxSpeed;
      }
      float relDist = dist / threshold;
      return relDist * maxSpeed;
   }

   protected void applyMovement(Point curTarget,float targetAngle, float targetSpeed, int delta) {
      // TURN
      if (FastMath.abs(targetAngle - angle) > ERR_ANGLE) {
         float toTurnAngle = rotationSpeed * delta;
         float dAngle = FastMath.fastAtan2(
                 FastMath.fastSin(targetAngle - angle), FastMath.fastCos(targetAngle - angle));
         if (rotationSpeed <= 0 || toTurnAngle > FastMath.abs(dAngle)) {
            toTurnAngle = dAngle;
         } else if (dAngle < 0) {
            toTurnAngle = -toTurnAngle;
         }
         angle += toTurnAngle + FastMath.TWO_PI;
         angle %= FastMath.TWO_PI;
      }
      //ACCEL
      if (FastMath.abs(targetSpeed - speed) > ERR_POS) {
         float toAccelSpeed = accel * delta;
         float dSpeed = targetSpeed - speed;
         if (accel <= 0 || toAccelSpeed > FastMath.abs(dSpeed)) {
            toAccelSpeed = dSpeed;
         } else if (dSpeed < 0) {
            toAccelSpeed = -toAccelSpeed;
         }
         speed += toAccelSpeed;
      }
      // MOVE
      if (FastMath.abs(speed) > ERR_POS) {
         float dX = FastMath.cos(angle) * speed * delta;
         float dY = -FastMath.sin(angle) * speed * delta;
         float toMoveX = curTarget.getX()-entity.getPos().getX();
         float toMoveY = curTarget.getY()-entity.getPos().getY();
         if (FastMath.abs(dX)>FastMath.abs(toMoveX)) {
            dX = toMoveX;
         }
         if (FastMath.abs(dY)>FastMath.abs(toMoveY)) {
            dY = toMoveY;
         }
         entity.getEnv().getEntitiesManager().popEntity(entity);
         entity.getPos().translate(dX, dY);
         entity.getBounds().translate(dX, dY);
         entity.getEnv().getEntitiesManager().pushEntity(entity);
      }
   }

   public void setEntity(Entity entity) {
      this.entity = entity;
   }

   public void setTarget(Point target) {
      this.target = target;
   }
}
